from copy import deepcopy
from geom2d.ray import Ray
from utils.misc import MAXFLOAT

from cranesched.routing.path import Path


class UnavoidableCollision(Exception):
    """This exception is raise when the given limit cannot be avoided. This occurs when a neighbor
    movable has a higher speed and is moving in the direction of the current movable (the one we're
    computing a bound for). In such cases, the current movable may not be able to move out of the
    neighbor's way on time."""


def compute_bound(path, limit, direction, speed, distance):
    """Given a path, a limit path, a distance, a movement direction, and a maximum movement speed,
    compute a movement bound for 'path'.
    The bound is obtained by moving as much as possible towards 'direction', while keeping a
    'distance' from the 'limit' and without exceeding the maximum movement 'speed' at all times.
    The limit path acts as a wall preventing the bound we're computing from reaching unauthorized
    positions. In the Crane Scheduling Problem, the movement limits of neighbor cranes or the
    warehouse's walls are used as limit to compute the movement bounds of cranes."""
    bound = Path(time=path.curr_time, position=(path.curr_position + direction*distance))
    if direction * bound.curr_position > direction * limit.position_at(bound.curr_time):
        raise UnavoidableCollision("planned paths lead to unavoidable collision")
    while bound.curr_time < limit.curr_time:
        index = limit.segment_indices_at(bound.curr_time)[-1]
        segment = limit.segments[index]
        if (bound.curr_position != segment.y_at(bound.curr_time) or
            direction > 0 and segment.m > speed or
            direction < 0 and segment.m < -speed):
            _approach_limit(bound, limit, direction, speed)
        elif abs(segment.m) <= speed:
            _follow_limit(bound, limit, speed, index, segment)
        else:
            _avoid_limit(bound, segment, direction, speed)
    assert bound.curr_position == limit.curr_position
    bound.shift_position(-direction * distance)
    full_bound = deepcopy(path)
    full_bound.extend(bound)
    return full_bound


def _approach_limit(bound, limit, direction, speed):
    """Approach the limit path at full speed."""
    assert direction * bound.curr_position <= direction * limit.position_at(bound.curr_time)
    # find a delta x that is roughly the magnitude of the path's time and position
    dx = (abs(bound.curr_time) + abs(bound.curr_position) + 1.0) / 2.0
    approach_ray = Ray((bound.curr_time, bound.curr_position),
                       (bound.curr_time + dx, bound.curr_position + dx * speed * direction))
    for (time, position), _ in limit.intersections(approach_ray):
        if time > bound.curr_time:
            bound.append(time, position)
            assert abs(bound.segments[-1].m) <= speed
            return
    # an intersection with the limit at a time greater than the bound's current time should always
    # occur if the limit is properly completed with a final horizontal segment representing the
    # position remaining the same indefinitely
    if limit.curr_time >= MAXFLOAT:
        raise Exception("hey! you're not supposed to be here... go away!")
    # this point should only be reached when the approach ray does not intersect the limit because
    # the slope of the limit's last segment is >= speed (for direction == +1). If the limit ends
    # before intersecting with the approach ray, we must manually intersect the approach ray with
    # a horizontal ray representing the limit staying in the same (last) position indefinitely.
    horizontal_ray = Ray((limit.curr_time, limit.curr_position),
                         (limit.curr_time * 2.0, limit.curr_position))
    time, position = horizontal_ray.intersection(approach_ray)
    bound.append(time, position)
    assert abs(bound.segments[-1].m) <= speed


def _follow_limit(bound, limit, speed, index, segment):
    """Follow the limit Path while the slope of its segments is less than or equal to the maximum
    movement speed specified."""
    while abs(segment.m) <= speed:
        bound.append(segment.x1, segment.y1)
        assert abs(bound.segments[-1].m) <= speed
        index += 1
        if index == len(limit.segments):
            break
        segment = limit.segments[index]


def _avoid_limit(bound, segment, direction, speed):
    """The object is unable to follow limit (we only know about the limit's next segment), so we
    must compute a trajectory that avoids crashing into the limit and merge it back with the
    bound that we built up to this point."""
    # find a delta x that is roughly the magnitude of the segment's x1 and y1
    dx = (abs(segment.x1) + abs(segment.y1) + 1.0) / 2.0
    avoid_ray = Ray((segment.x1, segment.y1),
                    (segment.x1 - dx, segment.y1 + dx * direction * speed))
    try:
        intersection = bound.intersection(avoid_ray, index=-1)  # fetch the right-most intersection
    except IndexError:
        raise UnavoidableCollision("impossible to avoid neighbor bound "
                                   "without modifying previously planned path")
    (time, position), index = intersection
    while bound.curr_time > time:
        bound.pop()
    if bound.append(time, position):
        assert abs(bound.segments[-1].m) <= speed
    bound.append(segment.x1, segment.y1)
    assert abs(bound.segments[-1].m) <= speed
